package legoproject;

import lejos.nxt.*;
import lejos.nxt.comm.*;
import java.io.*;

import legoproject.TestStubs.*;


public class GCSlave {
  //deployment
    public static final MotorPort grabMotorPort = MotorPort.B;
    public static final MotorPort towerMotorPort = MotorPort.A;
    public static final Motor grabMotor = Motor.B;
    public static final Motor towerMotor = Motor.A;
    public static final Button escapeButton = lejos.nxt.Button.ESCAPE;
    public static final SensorPort port1 = SensorPort.S1;
    public static final SensorPort touchSensorsPort = SensorPort.S2;
    public static final SensorPort Port4 = SensorPort.S4;
    public static final LightSensor towerLightSensor = new LightSensor(SensorPort.S3);
    public static final StatusReporter reporter = new NXTReporter();
    public static final ConnectionStarter starter = new ConnectionStarter();


  //testing
  /*  public static final PrintReporter reporter = new PrintReporter("Slave");
    public static final MotorPortTestStub grabMotorPort = new MotorPortTestStub();
    public static final MotorPortTestStub towerMotorPort = new MotorPortTestStub();
    public static final MotorTestStub grabMotor = new MotorTestStub();
    public static final MotorTestStub towerMotor = new MotorTestStub();
    public static final EscapeButtonTestStub escapeButton = new EscapeButtonTestStub("escapeButton", reporter);
    public static final LightSensorTestStub lightSensor = new LightSensorTestStub("lightSensor", reporter);
    public static final UltrasonicSensorTestStub towerLightSensor = new UltrasonicSensorTestStub("towerLightSensor", reporter);
    public static final ConnectionStarterTestStub starter = new ConnectionStarterTestStub();
   */


    private static final int fullTowerTurn = 3100;
    private static boolean foundHome = false;
    public static int lightThreshold = 1000;
    private final static int forward = 1,  backward = 2,  stop = 3,  flt = 4;
    private static HomeFinder homeFinder;
    private static boolean grabClosed = false;
    private static int highestValueSeenAt;
    private static int highestValueSeen;
    private static GrabShaker shaker = null;
    private static GarbageDetector garbageDetector = null;
    private static InputReader inputReader = null;
    public static int touchPressed = 0;

    public static int sensorStart;

    public static void main(String args[]) throws Exception {
        towerLightSensor.setFloodlight(false);
        inputReader = new InputReader();
        inputReader.start();
        //grabMotor.setSpeed(900);
        //grabMotor.setPower(100);
        towerMotor.setPower(100);
        findHomeDirection();
        towerMotor.setPower(80);
        lightThreshold = highestValueSeen + 8;
        LCD.clear();
        LCD.drawInt(lightThreshold, 2, 2);
        LCD.refresh();
        grabMotor.setSpeed(900);
        grabMotor.setPower(100);
        starter.init();
        DataInputStream dis = starter.getInputStream();
        DataOutputStream dos = starter.getOutputStream();
        LCD.clear();
        LCD.refresh();

        sensorStart = GCSlave.touchSensorsPort.readRawValue();

        while (true) {
            if(!grabClosed){
                //shaker = new GrabShaker();
                garbageDetector = new GarbageDetector();
                //shaker.start();
                garbageDetector.start();
            }
            int command = dis.readInt();
            if(!grabClosed){
                //shaker.interrupt();
                garbageDetector.interrupt();
                Thread.yield();
                //while(shaker.isAlive()){}
                while(garbageDetector.isAlive()){}
            }

            if (command == CommandType.GRAB) {
                doGrab();
                dos.writeInt(20);
                dos.flush();

            } else if (command == CommandType.RELEASEGRAB) {
                releaseGrab();
                dos.writeInt(70);
                dos.flush();
            } else if (command == CommandType.READTOUCH) {
                dos.writeInt(touchPressed);
                dos.flush();
                touchPressed = 0;

            } else if (command == CommandType.FINDHOME) {
                int res = findHomeDirection();
                dos.writeInt(res);
                dos.flush();
            } else if (command == CommandType.SHUTDOWN) {
                dos.writeInt(0);
                dos.flush();
                dis.close();
                dos.close();
                Thread.sleep(100); // wait for data to drain
                starter.closeConnection();
                releaseGrab();
                starter.init();
                dis = starter.getInputStream();
                dos = starter.getOutputStream();
            }
        //dis.close();
        //dos.close();
        //Thread.sleep(100); // wait for data to drain
        //btc.close();
        }


    }

    /**
     * HomeFinder calls this when home is found
     */
    public static void foundHome(){
        foundHome = true;
        towerMotor.stop();
    }

    public static void setHighestValueSeenAt(int highestValue){
        highestValueSeen = highestValue;
        highestValueSeenAt = towerMotor.getTachoCount();
    }

    public static void sensorPressed(){
        //shaker.interrupt();
        //doGrab();
        //garbageDetector.interrupt();
        touchPressed = 1;
    }

    private static int findHomeDirection(){
        highestValueSeenAt = -1;
        int res = -300;
        foundHome=false;
        homeFinder = new HomeFinder();
        homeFinder.start();
        towerMotor.rotate(fullTowerTurn/2, true);
        while(towerMotor.isRotating() && !foundHome){
            Thread.yield();
        }


        if(!foundHome){
            homeFinder.interrupt();
            homeFinder = new HomeFinder();
            homeFinder.start();
            towerMotor.rotate(-fullTowerTurn, true);
            while(towerMotor.isRotating() && !foundHome){
                Thread.yield();
            }
        }

        if(!foundHome){
            homeFinder.interrupt();
            homeFinder = new HomeFinder();
            homeFinder.start();
            towerMotor.rotate(fullTowerTurn/2, true);
            while(towerMotor.isRotating() && !foundHome){
                Thread.yield();
            }
        }

        if(foundHome){
            homeFinder.interrupt();
            int tachoCount = towerMotorPort.getTachoCount();
            res = (tachoCount*360)/fullTowerTurn;
            towerMotor.rotate(-towerMotor.getTachoCount(), true);
            return res;
        }
        homeFinder.interrupt();
        res = (highestValueSeenAt*360)/fullTowerTurn;
        towerMotor.rotate(-towerMotor.getTachoCount(), true);
        return res;
    }


    private static void doGrab() {

        //grabMotor.setPower(75);
        if(!grabClosed){
            grabClosed = true;
            grabMotor.rotate(-2700, false);
        }
    }

    private static void releaseGrab() {
        if(grabClosed){
            grabClosed = false;
            grabMotor.rotate(-grabMotor.getTachoCount(), false);
        }
        //grabMotor.setPower(100);
    }


    /* private static void print(String s) throws Exception {
    LCD.clear();
    LCD.drawString(s, 0, 0);
    LCD.refresh();
    //Thread.sleep(1000);
    }*/
}



class ConnectionStarter{
    private BTConnection btc;
    public void init(){
        btc = Bluetooth.waitForConnection();
    }
    public DataInputStream getInputStream(){
        return btc.openDataInputStream();
    }
    public DataOutputStream getOutputStream(){
        return btc.openDataOutputStream();
    }

    public void closeConnection(){
        btc.close();
        try{Thread.sleep(50);}catch(Exception e){}
    }

}

class HomeFinder extends Thread{

    public void run() {
        int highestValueSeen = -1;
        int value;
        while(true){
            value = GCSlave.towerLightSensor.readValue();
            if(value > GCSlave.lightThreshold){
                GCSlave.foundHome();
                return;
            }
            if(value > highestValueSeen){
                highestValueSeen = value;
                GCSlave.setHighestValueSeenAt(highestValueSeen);
            }
            try{Thread.sleep(100);}catch(Exception e){
                return;
            }

        }
    }
}

class GrabShaker extends Thread{
    public void run(){
        while(true){
            GCSlave.grabMotor.rotateTo(-30,true);
            while(GCSlave.grabMotor.isRotating()){
                try{Thread.sleep(10);}
                catch(Exception e){
                    GCSlave.grabMotor.rotateTo(0);
                    return;
                }
            }
            GCSlave.grabMotor.rotateTo(0,true);
            while(GCSlave.grabMotor.isRotating()){
                try{Thread.sleep(10);}
                catch(Exception e){
                    GCSlave.grabMotor.rotateTo(0);
                    return;
                }
            }
        }
    }
}

class GarbageDetector extends Thread{

    private boolean sensorPressed(){
        int value = GCSlave.touchSensorsPort.readRawValue();
        return value < GCSlave.sensorStart - 2;
    }

    public void run(){
        while(true){
            if(sensorPressed()){
                GCSlave.sensorPressed();
            }
            try{Thread.sleep(1);}
            catch(Exception e){
                return;
            }
        }
    }

}


class InputReader extends Thread{
    public void run(){
        GCSlave.reporter.nameStat(0, "light");
        GCSlave.reporter.nameStat(1, "touchers");
        GCSlave.reporter.nameStat(2, "port1");
        GCSlave.reporter.nameStat(3, "port4");
        while(true){
            int light = GCSlave.towerLightSensor.readValue();
            int touchers = GCSlave.touchSensorsPort.readRawValue();
            int port1 = GCSlave.port1.readRawValue();
            int port4 = GCSlave.port1.readRawValue();

            GCSlave.reporter.updateStat(0, light);
            GCSlave.reporter.updateStat(1, touchers);
            GCSlave.reporter.updateStat(2, port1);
            GCSlave.reporter.updateStat(3, port4);
            GCSlave.reporter.updateLCD();
            try{Thread.sleep(100);}catch(Exception e){}

        }
    }

}
